#include "FnSurfaceHarmonicParam.h"
#include <cmath>
#include "./taucs/taucs_fix.h"

int FnSurfaceHarmonicParam::nx0_face_side_idx = -1;
int FnSurfaceHarmonicParam::px0_face_side_idx = -1;
int FnSurfaceHarmonicParam::nx1_face_side_idx = -1;
int FnSurfaceHarmonicParam::px1_face_side_idx = -1;
int FnSurfaceHarmonicParam::ny_face_side_idx = -1;
int FnSurfaceHarmonicParam::py_face_side_idx = -1;
int FnSurfaceHarmonicParam::nz0_face_side_idx = -1;
int FnSurfaceHarmonicParam::pz0_face_side_idx = -1;
int FnSurfaceHarmonicParam::nz1_face_side_idx = -1;
int FnSurfaceHarmonicParam::pz1_face_side_idx = -1;


FnSurfaceHarmonicParam::FnSurfaceHarmonicParam(Mesh& _mesh)
  : mesh_(_mesh), weight_type_(kUniform), side_idx_(-1)
{
  mesh().add_property(vprop_lns_idx_);

  mesh().get_property_handle(vprop_param_, "vprop_param");
  mesh().get_property_handle(eprop_w_, "eprop_surface_w");
  mesh().get_property_handle(fprop_side_idx_, "fprop_side_idx");
  mesh().get_property_handle(vprop_type_, "vprop_type");
  mesh().get_property_handle(eprop_type_, "eprop_type");  
}

FnSurfaceHarmonicParam::~FnSurfaceHarmonicParam(void)
{
  mesh().remove_property(vprop_lns_idx_);
}

void FnSurfaceHarmonicParam::init()
{
  Mesh::VertexIter            v_it;
  Mesh::EdgeIter              e_it;
  Mesh::FaceIter              f_it;
  Mesh::FaceVertexIter        fv_it;
  Mesh::HalfedgeHandle        heh[2];
  int                         lns_idx;

  // set linear system index (i.e., vprop_lns_idx_)
  for (v_it=mesh().vertices_begin(); v_it!=mesh().vertices_end(); ++v_it)
  {
    mesh().property(vprop_lns_idx_, v_it) = -1;
  }
  for (f_it=mesh().faces_begin(); f_it!=mesh().faces_end(); ++f_it)
  {
    if (mesh().property(fprop_side_idx_, f_it) != side_idx_)
      continue;

    for (fv_it=mesh().fv_iter(f_it); fv_it; ++fv_it)
      if (mesh().property(vprop_type_, fv_it) == kSideVertex)
        mesh().property(vprop_lns_idx_, fv_it) = -2;
  }

  lns_idx = 0;
  for (v_it=mesh().vertices_begin(); v_it!=mesh().vertices_end(); ++v_it)
  {
    // only specified side-vertices will be considered
    if (mesh().property(vprop_lns_idx_, v_it) == -2)    
      mesh().property(vprop_lns_idx_, v_it) = lns_idx++;
  }
}



void FnSurfaceHarmonicParam::calc()
{
  export_linear_system("a.dat", "b.dat");
  calc_with_taucs();

  char  str[256];
  sprintf(str, "%d.obj", side_idx_);
  save(str);
}

void FnSurfaceHarmonicParam::save(const std::string& _filename)
{
  FILE    *fp;
  int     vidx;

  fp = fopen(_filename.c_str(), "w");

  Mesh::VertexIter           v_it;
  Mesh::HalffaceIter         hf_it;
  Mesh::HalffaceVertexIter   hfv_it;
  Mesh::Point           p;
  Param                 uvw;
  OpenVolMesh::Vec2f    st;

  OpenVolMesh::VPropHandleT<int>  vprop_new_idx;

  mesh().add_property(vprop_new_idx);
  for (v_it=mesh().vertices_begin(); v_it!=mesh().vertices_end(); ++v_it)
    mesh().property(vprop_new_idx, v_it) = -2;

  for (hf_it=mesh().halffaces_begin(); hf_it!=mesh().halffaces_end(); ++hf_it)
  {
    if (!mesh().is_boundary(hf_it))
      continue;
    if (side_idx_ != mesh().property(fprop_side_idx_, mesh().face_handle(hf_it)))
      continue;

    for (hfv_it=mesh().hfv_iter(hf_it); hfv_it; ++hfv_it)
      mesh().property(vprop_new_idx, hfv_it) = -1;      // the vertices will be considered
  }

  // v
  vidx = 0;
  for (v_it=mesh().vertices_begin(); v_it!=mesh().vertices_end(); ++v_it)
  {
    if (mesh().property(vprop_new_idx, v_it) != -1)
      continue;

    mesh().property(vprop_new_idx, v_it) = vidx++;
    p = mesh().point(v_it);
    fprintf(fp, "v %f %f %f\n", p[0], p[1], p[2]);
  }

  // vt
  for (v_it=mesh().vertices_begin(); v_it!=mesh().vertices_end(); ++v_it)
  {
    vidx = mesh().property(vprop_new_idx, v_it);
    if (vidx < 0)
      continue;

    uvw = mesh().property(vprop_param_, v_it);

	if (side_idx_ == 0 || side_idx_ == 1)        { st[0] = uvw[1];   st[1] = uvw[2]; }
	else
	{
		int m = side_idx_ % 4;
		switch (m)
		{
		case 0:
			st[0] = uvw[1]; st[1] = uvw[2];
			break;
		case 1:
			st[0] = uvw[0]; st[1] = uvw[2];
			break;
		case 2:
			st[0] = uvw[1]; st[1] = uvw[2];
			break;
		case 3:
			st[0] = uvw[0]; st[1] = uvw[2];
			break;
		}
	}		

    fprintf(fp, "vt %f %f\n", st[0], st[1]);
  }

  for (hf_it=mesh().halffaces_begin(); hf_it!=mesh().halffaces_end(); ++hf_it)
  {
    if (!mesh().is_boundary(hf_it))
      continue;
    if (side_idx_ != mesh().property(fprop_side_idx_, mesh().face_handle(hf_it)))
      continue;

    fprintf(fp, "f");
    for (hfv_it=mesh().hfv_iter(hf_it); hfv_it; ++hfv_it)
    {
      vidx = mesh().property(vprop_new_idx, hfv_it);
      fprintf(fp, " %d/%d", 1+vidx, 1+vidx);
    }
    fprintf(fp, "\n");
  }
  fclose(fp);

  mesh().remove_property(vprop_new_idx);
}





void FnSurfaceHarmonicParam::export_linear_system(const std::string& _A_dat, const std::string& _b_dat)
{
  FILE  *fp_A, *fp_B;
  int     n;
  int     i, j;
  Scalar  w_ii, w_ij;
  Scalar  b_0, b_1, b_2;    

  Mesh::VertexIter      v_it;
  Mesh::VertexEdgeIter  ve_it;
  Mesh::VertexHandle    vh_j;


  fp_A = fopen(_A_dat.c_str(), "w");
  fp_B = fopen(_b_dat.c_str(), "w");
  
  n = 0;
  for (v_it=mesh().vertices_begin(); v_it!=mesh().vertices_end(); ++v_it)
  {
    if (mesh().property(vprop_lns_idx_, v_it) < 0)
      continue;
    ++n;
  }

  for (v_it=mesh().vertices_begin(); v_it!=mesh().vertices_end(); ++v_it)  
  {
    i = mesh().property(vprop_lns_idx_, v_it);
    if (i < 0)
      continue;

    Mesh::VertexHandle  vh_i = v_it;

    w_ii = 0;
    b_0 = 0;
    b_1 = 0;
    b_2 = 0;
    for (ve_it=mesh().ve_iter(v_it); ve_it; ++ve_it)
    {
      if (mesh().property(eprop_type_, ve_it) != kSideEdge)
        continue;

      w_ij = mesh().property(eprop_w_, ve_it);
      w_ii += w_ij;
            
      vh_j = mesh().to_vertex_handle(mesh().halfedge_handle(ve_it));
      if (vh_j == v_it)
        vh_j = mesh().from_vertex_handle(mesh().halfedge_handle(ve_it));

      j = mesh().property(vprop_lns_idx_, vh_j);
      if (j >= 0)           // set element of A
      {
        fprintf(fp_A, "%d %d %f\n", 1+i, 1+j, -w_ij);
      }
      else                  // set element of B
      {
        Param uvw = mesh().property(vprop_param_, vh_j);

        b_0 += w_ij*uvw[0];
        b_1 += w_ij*uvw[1]; 
        b_2 += w_ij*uvw[2];
      }
    }
    fprintf(fp_A, "%d %d %f\n", 1+i, 1+i, w_ii);
    fprintf(fp_B, "%d 1 %f\n", 1+i, b_0);
    fprintf(fp_B, "%d 2 %f\n", 1+i, b_1);
    fprintf(fp_B, "%d 3 %f\n", 1+i, b_2);
  }
  fclose(fp_A);
  fclose(fp_B);
}




int FnSurfaceHarmonicParam::calc_with_taucs()
{
  taucs_ccs_matrix  *A = NULL;
  taucs_double      *u, *v, *w;
  taucs_double      *bu, *bv, *bw;
  FILE*             fp_ijv;
  int               i, j, num_nz;
  int               n;
  int               rc;
  char*             ijv_filename = "taucs_matrix.ijv";

  printf("FnSurfaceHarmonicParam::calc_with_taucs()\n");

  Mesh::VertexIter            v_it;
  Mesh::EdgeIter              e_it;
  Mesh::VertexEdgeIter        ve_it;
  Mesh::HalfedgeHandle        heh[2];
  Mesh::VertexHandle          vh_j;

  // calc # of free vertices (i.e., n x n)
  n = 0;
  for (v_it=mesh().vertices_begin(); v_it!=mesh().vertices_end(); ++v_it)
  {
    if (mesh().property(vprop_lns_idx_, v_it) < 0)
      continue;
    ++n;
  }
  printf("n=%d\n", n);
   

  // fill the matrix A and bu, bv, bw;
  fp_ijv = fopen(ijv_filename, "w");
  bu = (taucs_double *) taucs_vec_create(n, TAUCS_DOUBLE);
  bv = (taucs_double *) taucs_vec_create(n, TAUCS_DOUBLE);
  bw = (taucs_double *) taucs_vec_create(n, TAUCS_DOUBLE);
  num_nz = 0;
  for (v_it=mesh().vertices_begin(); v_it!=mesh().vertices_end(); ++v_it)  
  {
    double  w_ii, w_ij;

    i = mesh().property(vprop_lns_idx_, v_it);
    if (i < 0)
      continue;

    w_ii = 0;
    bu[i] = 0;
    bv[i] = 0;
    bw[i] = 0;

    for (ve_it=mesh().ve_iter(v_it); ve_it; ++ve_it)
    {
      if (mesh().property(eprop_type_, ve_it) != kSideEdge)
        continue;

      w_ij = mesh().property(eprop_w_, ve_it);
      w_ii += w_ij;

      vh_j = mesh().to_vertex_handle(mesh().halfedge_handle(ve_it));
      if (vh_j == v_it)
        vh_j = mesh().from_vertex_handle(mesh().halfedge_handle(ve_it));

      j = mesh().property(vprop_lns_idx_, vh_j);
      if (j >= 0)         // set element of A
      {
        fprintf(fp_ijv, "%d %d %f\n", i+1, j+1, (float) -w_ij);
        ++num_nz;      
      }
      else                // set element of B
      {
        Param uvw = mesh().property(vprop_param_, vh_j);
        
        bu[i] += w_ij*uvw[0];
        bv[i] += w_ij*uvw[1];
        bw[i] += w_ij*uvw[2];
      }
    }
    fprintf(fp_ijv, "%d %d %f\n", i+1, i+1, (float) w_ii);
    ++num_nz;
  }
  fclose(fp_ijv);

  taucs_logfile("stdout");


  A = taucs_ccs_read_ijv(ijv_filename, TAUCS_SYMMETRIC | TAUCS_DOUBLE);
  u = (taucs_double *) taucs_vec_create(A->n, A->flags);
  v = (taucs_double *) taucs_vec_create(A->n, A->flags);
  w = (taucs_double *) taucs_vec_create(A->n, A->flags);
  for (i=0; i<n; ++i)   { u[i] = v[i] = w[i] = 0; }

  taucs_ccs_write_ijv(A, "test.ijv");     // just for debugging




  /////////////////////////////////////
  //// cg solver (it is working also!)
  /////////////////////////////////////
  //printf("cg solver\n");
  //
  //for (i=0; i<n; ++i)   { printf("bu[%3d] = %lf\n", i, bu[i]); }
  //for (i=0; i<n; ++i)   { printf("bv[%3d] = %lf\n", i, bv[i]); }
  //for (i=0; i<n; ++i)   { printf("bw[%3d] = %lf\n", i, bw[i]); }
  //
  //// computing u
  //rc = taucs_conjugate_gradients(A, NULL, NULL, u, bu, 2*A->n, 1e-7);
  //if (rc == TAUCS_SUCCESS)  printf("cg solver for computing u: it's ok\n");
  //else                      printf("cg solver for computing u: it's NOT ok\n");
  //// computing v
  //rc = taucs_conjugate_gradients(A, NULL, NULL, v, bv, 2*A->n, 1e-7);
  //if (rc == TAUCS_SUCCESS)  printf("cg solver for computing v: it's ok\n");
  //else                      printf("cg solver for computing v: it's NOT ok\n");
  //// computing w
  //rc = taucs_conjugate_gradients(A, NULL, NULL, w, bw, 2*A->n, 1e-7);
  //if (rc == TAUCS_SUCCESS)  printf("cg solver for computing w: it's ok\n");
  //else                      printf("cg solver for computing w: it's NOT ok\n");


  //////////////////////////////////////////////////////////////////////////
  //// LLT factor, solve, free the factorization
  //////////////////////////////////////////////////////////////////////////
  char* options_factor[] = {"taucs.factor.LLT=true", NULL };
  void* F = NULL;
  rc = taucs_linsolve(A, &F, 0, NULL, NULL, options_factor, NULL);    // factor
  if (rc != TAUCS_SUCCESS)  printf("problem in factor\n");
  rc = taucs_linsolve(A, &F, 1, u, bu, options_factor, NULL);        // computing u
  if (rc != TAUCS_SUCCESS)  printf("problem in solving x0\n");
  rc = taucs_linsolve(A, &F, 1, v, bv, options_factor, NULL);        // computing v
  if (rc != TAUCS_SUCCESS)  printf("problem in solving x1\n");
  rc = taucs_linsolve(A, &F, 1, w, bw, options_factor, NULL);        // computing w
  if (rc != TAUCS_SUCCESS)  printf("problem in solving x1\n");
  rc = taucs_linsolve(NULL, &F, 0, NULL, NULL, NULL, NULL);
  if (rc != TAUCS_SUCCESS)  printf("problem in free\n");


  // get results
  for (v_it=mesh().vertices_begin(); v_it!=mesh().vertices_end(); ++v_it)  
  {
    i = mesh().property(vprop_lns_idx_, v_it);
    if (i < 0)
      continue;

    Param   uvw;

    uvw[0] = (Scalar) u[i];
    uvw[1] = (Scalar) v[i];
    uvw[2] = (Scalar) w[i];

    mesh().property(vprop_param_, v_it) = uvw;
  }
  

  return  0;
}